/*
里程计模块
1.接收边缘点和平面点
2.发送里程计信息和
*/

#include <cmath>
#include "common.h"
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

//一个点云周期
const float scanPeriod = 0.1;

//控制给laserMapping发送频率
const int skipFrameNum = 1;
bool systemInited = false;

//时间戳信息
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;

//接收消息标志
bool newCornerPointsSharp = false;
bool newCornerPointsLessSharp = false;
bool newSurfPointsFlat = false;
bool newSurfPointsLessFlat = false;
bool newLaserCloudFullRes = false;

//接收边缘点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp (new pcl::PointCloud<PointType>());
//接收次边缘点
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp (new pcl::PointCloud<PointType>());
//接收平面点
pcl::PointCloud<PointType>::Ptr surfPointsFlat (new pcl::PointCloud<PointType>());
//接收次平面点
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat (new pcl::PointCloud<PointType>());
//接收所有点
pcl::PointCloud<PointType>::Ptr laserCloudFullRes (new pcl::PointCloud<PointType>());
//上一帧的次边缘点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast (new pcl::PointCloud<PointType>());
//上一帧的次平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast (new pcl::PointCloud<PointType>());

//kd-tree 上一帧的次边缘点 K大写
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
//kd-tree 上一帧的次平面点
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());

//保存前一个节点发过来的未经处理过的特征点  coefficient:系数
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());

//上一帧次边缘点和次平面点数量
int laserCloudCornerLastNum;
int laserCloudSurfLastNum;

//保存从上一帧找到的两个边缘点的索引
int pointSearchCornerInd1[40000];
int pointSearchCornerInd2[40000];
//保存从上一帧找到的三个边缘点的索引
int pointSearchSurfInd1[40000];
int pointSearchSurfInd2[40000];
int pointSearchSurfInd3[40000];

//当前帧相对上一帧的状态转移量，in the local frame 欧拉角表达旋转
float transform1[6] = {0};
//当前帧相对于第一帧的状态转移量，in the global frame
float transformSum[6] = {0};

void TransformToStart(PointType const* const pi, PointType* const po){
    //计算该点相对于第一个点时间比值 插值系数计算
    float s = 10 * (pi->intensity - int(pi->intensity));
    //线性插值 计算该点相对于第一个点的平移旋转关系
    float rx = s * transform1[0];
    float ry = s * transform1[1];
    float rz = s * transform1[2];
    float tx = s * transform1[3];
    float ty = s * transform1[4];
    float tz = s * transform1[5];

    //欧拉角表示旋转ZXY  ZXY转回来 P2 = RyRxRz*P1 + t12  P1 = (P2 - t12)乘逆RzRxRy
    //绕着Z旋转-rz 
    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = pi->z - tz;
    //绕着X轴旋转-rx
    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rz) * z1;
    float z2 = -sin(rx) * y1 + cos(rz) * z1;
    //绕着Y轴旋转-ry
    po->x = cos(ry) * x2 + sin(ry) * z2;
    po->y = y2;
    po->z = -sin(ry) * x2 + cos(ry) * z2;
    po->intensity = pi->intensity;

}

//转到一帧的最后一个坐标系
void TransformToEnd(PointType const* const pi, PointType* const po){
    //先把点转到起始坐标系
    PointType tmp;
    TransformToStart(pi, &tmp);
    //再将点转到最后一个点坐标系
    float s = 10 * (pi->intensity - int(pi->intensity));
    float rx = s * transform1[0];
    float ry = s * transform1[1];
    float rz = s * transform1[2];
    float tx = s * transform1[3];
    float ty = s * transform1[4];
    float tz = s * transform1[5];
    //按照ZXY转换到最后一个坐标系: P1 =  RzRxRy * P2 + t
    //绕着Y轴转ry
    float x1 = cos(ry) * tmp.x - sin(ry) * tmp.z;
    float y1 = tmp.y;
    float z1 = sin(ry) * tmp.x + cos(ry) * tmp.z;
    //绕着X轴旋转rx
    float x2 = x1;
    float y2 = cos(rx) * y1 - sin(rx) * z1;
    float z2 = sin(rx) * y1 + cos(rx) * z1;
    //绕着Z轴旋转rz后加上平移量
    po->x = cos(rz) * x2 - sin(rz) * y2 + tx;
    po->y = sin(rz) * x2 + cos(rz) * y2 + ty;
    po->z = z2 + tz;
    po->intensity = int(pi->intensity);
}

//状态量更新函数 
//输入上一时刻的状态量 和相对运动 输出:更新量
void statusUpdate(float* transformSum, float* transform ,Eigen::Quaterniond &q){
    //先将欧拉角转换成旋转向量
    Eigen::AngleAxisd  rx_sum(transformSum[0], Eigen::Vector3d(1,0,0));
    Eigen::AngleAxisd  ry_sum(transformSum[1], Eigen::Vector3d(0,1,0));
    Eigen::AngleAxisd  rz_sum(transformSum[2], Eigen::Vector3d(0,0,1));
    Eigen::Vector3d    t_sum(transformSum[0], transformSum[1], transformSum[2]);
    //将旋转向量转化成旋转矩阵 绕着ZXY顺序旋转 左乘
    Eigen::Matrix3d rotation_matrix_sum =  ry_sum.toRotationMatrix() * rx_sum.toRotationMatrix() * rz_sum.toRotationMatrix();

    //将相对变换转化成旋转矩阵
    Eigen::AngleAxisd  rx(transform[0], Eigen::Vector3d(1,0,0));
    Eigen::AngleAxisd  ry(transform[1], Eigen::Vector3d(0,1,0));
    Eigen::AngleAxisd  rz(transform[2], Eigen::Vector3d(0,0,1));
    Eigen::Vector3d    t(transform[0], transform[1], transform[2]);
    Eigen::Matrix3d rotation_matrix =  ry.toRotationMatrix() * rx.toRotationMatrix() * rz.toRotationMatrix();
    //更新位移量
    t_sum = t_sum + rotation_matrix_sum * t;
    //更新旋转矩阵
    rotation_matrix_sum = rotation_matrix_sum * rotation_matrix;
    //旋转矩阵转化成欧拉角
    Eigen::Vector3d eulerAngle = rotation_matrix_sum.eulerAngles(2,0,1);
    //旋转矩阵转四元数 
    q = rotation_matrix_sum;
    //更新状态量
    transformSum[0] = eulerAngle[1];
    transformSum[1] = eulerAngle[2];
    transformSum[2] = eulerAngle[0];
    transformSum[3] = t_sum[0];
    transformSum[4] = t_sum[1];
    transformSum[5] = t_sum[2];

}

//接收边缘点
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2 ){
    //获取时间戳
    timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec();
    cornerPointsSharp->clear();
    //转换数据类型
    pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp);
    std::vector<int> indices;
    //移除无效点
    pcl::removeNaNFromPointCloud(*cornerPointsSharp, *cornerPointsSharp, indices);
    newCornerPointsSharp = true;
}
//接收次边缘点
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2 ){

    timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec();
    cornerPointsLessSharp->clear();
    pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cornerPointsLessSharp, *cornerPointsLessSharp, indices);
    newCornerPointsLessSharp = true;
}
//接收平面点
void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2 ){
    
    timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec();
    surfPointsFlat->clear();
    pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*surfPointsFlat, *surfPointsFlat, indices);
    newSurfPointsFlat = true;
}
//接收次平面点
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2 ){

    timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec();
    surfPointsLessFlat->clear();
    pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*surfPointsLessFlat, *surfPointsLessFlat, indices);
    newSurfPointsLessFlat = true;
}
//接收所有点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2 ){

    timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();
    laserCloudFullRes->clear();
    pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*laserCloudFullRes, *laserCloudFullRes, indices);
    newLaserCloudFullRes = true;
}


int main(int argc, char** argv){
    //节点初始化
    ros::init(argc, argv, "laserOdometry");
    //节点句柄
    ros::NodeHandle nh;
    //接收边缘点数据
    ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2, laserCloudSharpHandler);
    //接收次边缘点数据
    ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);
    //接收平面点
    ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf", 2, laserCloudFlatHandler);
    //接收次平面点
    ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_surf", 2, laserCloudLessFlatHandler);
    //接收所有点
    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2, laserCloudFullResHandler);

    //发送上一帧的边缘点
    ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
    //发送上一帧的平面点
    ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
    //发送所有点
    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2);
    //发送里程计信息
    ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("laser_odom_to_init", 2);
    //里程计信息
    nav_msgs::Odometry laserOdometry;
    laserOdometry.header.frame_id = "/camera_init";
    laserOdometry.child_frame_id = "/laser_odom";
    //tf变换信息发布
    tf::TransformBroadcaster tfBroadcaster;
    tf::StampedTransform laserOdometryTrans;
    laserOdometryTrans.frame_id_ = "/camera_init";
    laserOdometryTrans.child_frame_id_ = "laser_odom";

    //搜索到的点序
    std::vector<int> pointSearchInd;
    //搜索到的点距离的平方
    std::vector<float> pointSearchSqDis;
    //选中的特征点
    PointType pointOri, pointSel;
    //特征点对应点
    PointType tripod1, tripod2, tripod3;
    //权重系数
    PointType coeff;

    //退化标志
    bool isDegenerate = false;
    //预测矩阵
    Eigen::Matrix<float, 6, 6>matP;
    //帧数计数 没两帧发送一次 次边缘点和次平面点
    int frameCount = 1;
    ros::Rate rate(100);
    bool status = ros::ok();

    while(status){
        //开始执行回调函数
        ros::spinOnce();
        //确保接收到的数据在同一时间 也就是位于消息队列同一位置
        if(newCornerPointsSharp && newCornerPointsLessSharp && 
           newSurfPointsFlat && newSurfPointsLessFlat && newLaserCloudFullRes &&
           fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
           fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
           fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
           fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005){
               newCornerPointsSharp = false;
               newCornerPointsLessSharp = false;
               newSurfPointsFlat = false;
               newSurfPointsLessFlat = false;
               newLaserCloudFullRes = false;
               //将第一个点云发送给laserMapping 从第二个点云开始处理
               if(!systemInited){
                   //将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用
                   pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
                   cornerPointsLessSharp = laserCloudCornerLast;
                   laserCloudCornerLast = laserCloudTemp;
                   //将surfPointsLessFlat与laserCloudSurfLast交换 目的是保存surfPointsLessFlat的值下一轮使用
                   laserCloudTemp = surfPointsLessFlat;
                   surfPointsLessFlat = laserCloudSurfLast;
                   laserCloudSurfLast = laserCloudTemp;

                   std::cout<<"size corner:"<<laserCloudCornerLast->points.size()<<endl;
                   std::cout<<"size surf:"<<  laserCloudSurfLast->points.size()<<endl;

                   //使用第一帧的点构建kd-tree
                   kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
                   kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
                   //将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMapping
                   sensor_msgs::PointCloud2 laserCloudCornerLast2;
                   pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                   laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                   laserCloudCornerLast2.header.frame_id = "/camera";
                   pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

                   sensor_msgs::PointCloud2 laserCloudSurfLast2;
                   pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                   laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                   laserCloudSurfLast2.header.frame_id = "/camera";
                   pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

                   systemInited = true;
                   std::cout<<"Initialization Finished!"<<endl;
                   continue;
               }//初始化完毕

               //上一帧的角点和平面点的数量
               laserCloudCornerLastNum = laserCloudCornerLast->points.size();
               laserCloudSurfLastNum   = laserCloudSurfLast->points.size();

               //开始进行匹配
               if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100){
                   //点序号
                   std::vector<int> indices;
                   pcl::removeNaNFromPointCloud(*cornerPointsSharp, *cornerPointsSharp, indices);
                   int cornerPointsSharpNum = cornerPointsSharp->points.size();
                   int surfPointsFlatNum = surfPointsFlat->points.size();

                   //使用L-M方法进行迭代计算
                   for (int itercount = 0; itercount < 25; itercount++){
                       laserCloudOri->clear();
                       coeffSel->clear();

                       //寻找角点对应点 一个是欧氏距离最近 另一个在相邻线上找
                       for (int i = 0; i < cornerPointsSharpNum; i++){
                           //将point[i]转到第一个点的坐标系 这里假设相邻帧之间是匀速运动
                           TransformToStart(&cornerPointsSharp->points[i], &pointSel);
                           //每迭代五次重新查找最近点 查找最近点
                           if(itercount % 5 == 0){
                               std::vector<int> indices;
                               //1.输入点云   2.输出点云   3.索引
                               pcl::removeNaNFromPointCloud(*laserCloudCornerLast, *laserCloudCornerLast, indices);
                               //使用k-dtree寻找第一个最近点
                               kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                               //最近点寻找标志
                               int closestPointInd = -1, minPointInd2 = -1;
                               //寻找相邻线距离目标点距离最小的点 第一个对应点
                               //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
                               if(pointSearchSqDis[0] < 25 ){//找到的最近点距离的确很近的话
                                   closestPointInd = pointSearchInd[0];
                                   //提取最近点线号
                                   int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);
                                   //寻找第二个点
                                   //初始门槛5米 可过滤掉大部分点
                                   //和最近点的距离平方以及设定距离阈值
                                   float pointSqDis, minPointSqDis2 = 25;
                                   //上半部分寻找第二近点
                                   for(int j = closestPointInd + 1; j < cornerPointsSharpNum; j++){
                                       //非相邻线
                                       if(int(laserCloudCornerLast->points[j].intensity > closestPointScan + 2.5)){
                                           break;
                                       }
                                       pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                    (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                    (laserCloudCornerLast->points[j].z - pointSel.z);
                                       //确保不在一条线上
                                       if(int(laserCloudCornerLast->points[j].intensity > closestPointScan)){
                                           //找最近距离
                                           if(pointSqDis < minPointSqDis2){
                                               minPointSqDis2 = pointSqDis;
                                               minPointInd2 = j;
                                           }       
                                       } 
                                   }//上半部分寻找结束
                                   //同理寻找下半部分
                                   for(int j = closestPointInd - 1; j >= 0; j--){
                                       if(int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5){
                                           break;
                                       }
                                       pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                    (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) ;
                                       if(int(laserCloudCornerLast->points[j].intensity) < closestPointScan){
                                           if(pointSqDis < minPointSqDis2){
                                               minPointSqDis2 = pointSqDis;
                                               minPointInd2 = j;
                                           }
                                       } 
                                   }//下半部分寻找结束                              
                               }//角点第二个点寻找结束

                               //记住组成线的两个点的点序 最近点和次近点
                               pointSearchCornerInd1[i] = closestPointInd;
                               pointSearchCornerInd2[i] = minPointInd2;
                           }//查找最近点结束
                           //大于等于0，不等于-1，说明两个点都找到了
                           if(pointSearchCornerInd2[i] >= 0){
                               tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                               tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                               //选择的特征点记为X0 最近点记为X1 次近点记为X2
                               float x0 = pointSel.x;
                               float y0 = pointSel.y;
                               float z0 = pointSel.z;
                               float x1 = tripod1.x;
                               float y1 = tripod1.y;
                               float z1 = tripod1.z;
                               float x2 = tripod2.x;
                               float y2 = tripod2.y;
                               float z2 = tripod2.z;

                               //向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）
                               //向量OA OB的向量积(即叉乘)为：
                               //|  i      j      k  |
                               //|x0-x1  y0-y1  z0-z1|
                               //|x0-x2  y0-y2  z0-z2|
                               //模为：
                               float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                                            * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                            + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                                            * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                            + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                                            * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
                               //两个最近点之间的距离为
                               float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
                               //点到线的距离为
                               float ld2 = a012 / l12;
                               //d方向归一化后在各轴的分量为
                               //x轴分量i
                               float la = ((z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 -z1))+
                                          (y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))) / ld2;
                               //y轴分量j
                               float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) 
                                          -(z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / ld2;
                               //z轴分量k
                               float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) 
                                          +(y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / ld2; 
                               //权重计算 距离越大权重越小 距离越小权重越大 
                               float s = 1;
                               //5次迭代之后开始增加权重因素
                               if(itercount >= 5){
                                   s = 1 - 1.8 * fabs(ld2);
                               }
                               //考虑权重
                               coeff.x = s * la;
                               coeff.y = s * lb;
                               coeff.z = s * lc;
                               coeff.intensity = s * ld2;

                               //保留权重大的并且距离为0的点去掉
                               if( s > 0.1 && ld2 != 0){
                                   laserCloudOri->push_back(cornerPointsSharp->points[i]);
                                   coeffSel->push_back(coeff);
                               }
                           }
                       }//角点对应点寻找结束

                       //开始寻找平面对应点
                       for(int i = 0; i < surfPointsFlatNum; i++){
                           TransformToStart(&surfPointsFlat->points[i], &pointSel);
                           //每迭代5次重新寻找对应点
                           if(itercount % 5 == 0){
                               //k-dtree寻找最近点
                               kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                               //是否找到标识
                               int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                               if(pointSearchSqDis[0] < 25){ //找到对应点且很近就寻找第二第三个点
                                   //该点的索引
                                   closestPointInd = pointSearchInd[0];
                                   //该点的线号
                                   int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);
                                   //开始寻找第二和第三个点
                                   float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
                                   //上半部分查找
                                   for(int j = closestPointScan + 1; j < laserCloudSurfLastNum; j++){
                                       if(int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5){
                                           break;
                                       }
                                       //计算每个点与最近点的距离平方
                                       pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                                    (laserCloudSurfLast->points[j].x - pointSel.x) +
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) *
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) +
                                                    (laserCloudSurfLast->points[j].z - pointSel.z) *
                                                    (laserCloudSurfLast->points[j].z - pointSel.z);
                                       //找同一个线号上的点
                                       if(int(laserCloudSurfLast->points[j].intensity) == closestPointScan){
                                           if(pointSqDis < minPointInd2){
                                               minPointSqDis2 = pointSqDis;
                                               minPointInd2 = j;
                                           }
                                       }
                                       //找相邻线上的3号点
                                       else{
                                           if(pointSqDis < minPointSqDis3){
                                               minPointSqDis3 = pointSqDis;
                                               minPointInd3 = j;
                                           }
                                       }                                         
                                   }//上半部分查找结束
                                   //下半部分查找开始
                                   for(int j = closestPointInd - 1; j >= 0; j++){
                                       if(int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5){
                                           break;
                                       }
                                       pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                                    (laserCloudSurfLast->points[j].x - pointSel.x) +
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) *
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) +
                                                    (laserCloudSurfLast->points[j].z - pointSel.z) *
                                                    (laserCloudSurfLast->points[j].z - pointSel.z);
                                       //查找2号点
                                       if(int(laserCloudSurfLast->points[j].intensity) == closestPointScan){
                                           if(pointSqDis < minPointSqDis2){
                                               minPointInd2 = pointSqDis;
                                               minPointInd2 = j;
                                           }
                                       }
                                       //查找三号店
                                       else{
                                           if(pointSqDis < minPointSqDis3){
                                               minPointSqDis3 = pointSqDis;
                                               minPointInd3 = j;
                                           }
                                       }
                                   }//下半部分查找结束
                               }//第二第三个点寻找结束
                               //把该点的对应点存储
                               pointSearchSurfInd1[i] = closestPointInd;
                               pointSearchSurfInd2[i] = minPointInd2;
                               pointSearchSurfInd3[i] = minPointInd3;
                           }//if(iterator % 5 == 0)
                           //三个点都找到了
                           if(pointSearchSurfInd2[i] >=0 && pointSearchSurfInd3[i] >= 0){
                               tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//A点
                               tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//B点
                               tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//C点

                               //向量AB = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
                               //向量AC = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)

                               //向量AB AC的向量积（即叉乘），得到的是法向量
                               //x轴方向分向量i
                               float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 
                                        - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
                               //y轴方向分向量j
                               float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 
                                        - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
                               //z轴方向分向量k
                               float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 
                                        - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
                               float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
                               //法向量的模
                               float ps = sqrt(pa * pa + pb * pb + pc * pc);
                               //pa pb pc为法向量的单位向量
                               pa /= ps;
                               pb /= ps;
                               pc /= ps;
                               pd /= ps;

                               //点到平面的距离：向量OA与与法向量的点积除以法向量的模
                               //d = Pa*(pointSel.x - tripod1) + Pb*(pointSel.y - tripod2) + Pc*(pointSel.z - tripod3)
                               float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
                               
                               //计算权重
                               float s = 1;
                               if(itercount >= 5){
                                   s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                                     + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
                               }
                               //考虑权重
                               coeff.x = s * pa;
                               coeff.y = s * pb;
                               coeff.z = s * pc;
                               coeff.intensity = s * pd2;
                               
                               //保存原始点和系数
                               if(s > 0.1 && s != 0){
                                   laserCloudOri->push_back(surfPointsFlat->points[i]);
                                   coeffSel->push_back(coeff);
                               }
                           }

                       }//平面对应点寻找结束

                       int pointSelNum = laserCloudOri->points.size();
                       //如果满足的特征点少于10个，弃用此帧数据
                       if(pointSelNum <10 ){
                           continue;
                       }
                       //开始求解
                       Eigen::Matrix<float,Eigen::Dynamic,6> matA(pointSelNum, 6);
                       Eigen::Matrix<float,6,Eigen::Dynamic> matAt(6, pointSelNum);
                       Eigen::Matrix<float,6,6> matAtA;
                       Eigen::VectorXf matB(pointSelNum);
                       Eigen::Matrix<float,6,1> matAtB;
                       Eigen::Matrix<float,6,1> matX;
                       //构建雅克比矩阵求解matAtA和matAtB
                       for(int i = 0; i < pointSelNum; i++){
                           pointOri = laserCloudOri->points[i];
                           coeff = coeffSel->points[i];

                           float s = 1;
                           //计算使用
                           float srx = sin(transform1[0]);
                           float crx = cos(transform1[0]);
                           float sry = sin(transform1[1]);
                           float cry = cos(transform1[1]);
                           float srz = sin(transform1[2]);
                           float crz = cos(transform1[2]);
                           float tx = s * transform1[3];
                           float ty = s * transform1[4];
                           float tz = s * transform1[5];

                           //计算雅克比矩阵J
                           //对rx求导
                           float arx = (-s * crx*sry*srz*pointOri.x + s * crx*crz*sry*pointOri.y + s * srx*sry*pointOri.z
                                        + s * tx*crx*sry*srz - s * ty*crx*crz*sry - s * tz*srx*sry) * coeff.x
                                        + (s*srx*srz*pointOri.x - s * crz*srx*pointOri.y + s * crx*pointOri.z
                                        + s * ty*crz*srx - s * tz*crx - s * tx*srx*srz) * coeff.y
                                        + (s*crx*cry*srz*pointOri.x - s * crx*cry*crz*pointOri.y - s * cry*srx*pointOri.z
                                        + s * tz*cry*srx + s * ty*crx*cry*crz - s * tx*crx*cry*srz) * coeff.z;
                           //对ry求导
                           float ary = ((-s * crz*sry - s * cry*srx*srz)*pointOri.x
                                        + (s*cry*crz*srx - s * sry*srz)*pointOri.y - s * crx*cry*pointOri.z
                                        + tx * (s*crz*sry + s * cry*srx*srz) + ty * (s*sry*srz - s * cry*crz*srx)
                                        + s * tz*crx*cry) * coeff.x
                                        + ((s*cry*crz - s * srx*sry*srz)*pointOri.x
                                        + (s*cry*srz + s * crz*srx*sry)*pointOri.y - s * crx*sry*pointOri.z
                                        + s * tz*crx*sry - ty * (s*cry*srz + s * crz*srx*sry)
                                        - tx * (s*cry*crz - s * srx*sry*srz)) * coeff.z;
                           //对rz求导
                           float arz = ((-s * cry*srz - s * crz*srx*sry)*pointOri.x + (s*cry*crz - s * srx*sry*srz)*pointOri.y
                                        + tx * (s*cry*srz + s * crz*srx*sry) - ty * (s*cry*crz - s * srx*sry*srz)) * coeff.x
                                        + (-s * crx*crz*pointOri.x - s * crx*srz*pointOri.y
                                        + s * ty*crx*srz + s * tx*crx*crz) * coeff.y
                                        + ((s*cry*crz*srx - s * sry*srz)*pointOri.x + (s*crz*sry + s * cry*srx*srz)*pointOri.y
                                        + tx * (s*sry*srz - s * cry*crz*srx) - ty * (s*crz*sry + s * cry*srx*srz)) * coeff.z;
                            //对tx求导
                            float atx = -s * (cry*crz - srx * sry*srz) * coeff.x + s * crx*srz * coeff.y
                                        - s * (crz*sry + cry * srx*srz) * coeff.z;
                            //对ty求导
                            float aty = -s * (cry*srz + crz * srx*sry) * coeff.x - s * crx*crz * coeff.y
                                        - s * (sry*srz - cry * crz*srx) * coeff.z;
                            //对tz求导
                            float atz = s * crx*sry * coeff.x - s * srx * coeff.y - s * crx*cry * coeff.z;
                            //误差f(x)
                            float d2 = coeff.intensity;

                            //构建雅克比矩阵
                            matA(i, 0) = arx;
                            matA(i, 1) = ary;
                            matA(i, 2) = arz;
                            matA(i, 3) = atx;
                            matA(i, 4) = aty;
                            matA(i, 5) = atz;
                            matB(i, 0) = -0.05 * d2; 
                           
                       }
                       //求解
                       matAt = matA.transpose();
                       matAtA = matAt * matA;
                       matAtB = matAt * matB;
                       //求解 AtA * x = AtB;
                       matX = matAtA.colPivHouseholderQr().solve(matAtB);

                       //TODO:矩阵退化处理 不懂
                       if (itercount == 0){
                           Eigen::Matrix<float, 1, 6> matE;
                           Eigen::Matrix<float, 6, 6> matV;
                           Eigen::Matrix<float, 6, 6> matV2;

                           //计算特征值和特征向量
                           Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
                           matE = esolver.eigenvalues().real();
                           matV = esolver.eigenvectors().real();

                           matV2 = matV;

                           isDegenerate = false;
                           float eigenThre[6] = {10,10,10,10,10,10};
                           for(int i = 0; i < 6; i++){
                               if(matE(0, i) < eigenThre[i]){
                                   for(int j = 0; j < 6; j++){
                                       matV2(i , j) = 0;
                                   }
                                   isDegenerate = true;
                               }
                               else{
                                   break;
                               }

                           }
                           //计算P矩阵
                           matP = matV.inverse() * matV2;
                       }
                       //if
                       if(isDegenerate){
                           Eigen::Matrix<float, 6, 1> matX2(matX);
                           matX = matP * matX2;
                       }

                       //状态量更新
                       transform1[0] += matX(0, 0);
                       transform1[1] += matX(1, 0);
                       transform1[2] += matX(2, 0);
                       transform1[3] += matX(3, 0);
                       transform1[4] += matX(4, 0);
                       transform1[5] += matX(5, 0);

                       //判断是否非数字
                       for(int i = 0; i< 6; i++){
                           if(isnan(transform1[i])){
                               transform1[i] = 0;
                           }
                       }

                       //计算旋转平移量 如果很小就停止迭代
                       float deltaR = sqrt( pow(rad2deg(matX(0,0)), 2) + 
                                            pow(rad2deg(matX(1,0)), 2) +
                                            pow(rad2deg(matX(2,0)), 2) );
                       float deltaT = sqrt(
                                            pow(matX(3, 0) * 100, 2) +
                                            pow(matX(4, 0) * 100, 2) +
                                            pow(matX(5, 0) * 100, 2));
                       
                       //如果足够小
                       if(deltaR < 0.1 && deltaT < 0.1){
                           break;
                       }
                   }//L-M迭代结束
               }//结束匹配

               //将相对坐标转化成绝对坐标
               Eigen::Quaterniond q;
               statusUpdate(transformSum , transform1, q);

               //发布相对于世界坐标系的位置和姿态
               laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
               laserOdometry.pose.pose.orientation.x = q.coeffs().x();
               laserOdometry.pose.pose.orientation.y = q.coeffs().y();
               laserOdometry.pose.pose.orientation.z = q.coeffs().z();
               laserOdometry.pose.pose.orientation.w = q.coeffs().w();
               laserOdometry.pose.pose.position.x = transformSum[3];
               laserOdometry.pose.pose.position.y = transformSum[4];
               laserOdometry.pose.pose.position.z = transformSum[5];
               pubLaserOdometry.publish(laserOdometry);
               
               //广播平移旋转之后的坐标系
               laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat);
               laserOdometryTrans.setRotation(tf::Quaternion(q.coeffs().x(),q.coeffs().y(),q.coeffs().z(),q.coeffs().w()));
               laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3],transformSum[4],transformSum[5]));
               tfBroadcaster.sendTransform(laserOdometryTrans);
               //将曲率较大和比较小的点投影到扫描结束位置
               int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
               for(int i = 0; i < cornerPointsLessSharpNum; i++){
                   TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
               }

               int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
               for (int i = 0; i < surfPointsLessFlatNum; i++) {
                   TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
               }
               //帧数加1
               frameCount++;
               //点云全部点，每隔一个点云数据相对于最后一个点进行畸变矫正
               if(frameCount >= skipFrameNum + 1){
                   int laserCloudFullResNum = laserCloudFullRes->points.size();
                   for(int i = 0; i< laserCloudFullResNum; i++){
                       TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
                   }
               }

               //畸变校正后的点作为last点保存等待下个点云进来匹配
               pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
               cornerPointsLessSharp = laserCloudCornerLast;
               laserCloudCornerLast = laserCloudTemp;

               laserCloudTemp = surfPointsLessFlat;
               surfPointsLessFlat = laserCloudSurfLast;
               laserCloudSurfLast = laserCloudTemp;
               
               laserCloudCornerLastNum = laserCloudCornerLast->points.size();
               laserCloudSurfLastNum = laserCloudSurfLast->points.size();
               //点足够多就构建kd-tree，否则弃用此帧，沿用上一帧数据的kd-tree
               if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
                    kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
                    kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
               }
               //按照跳帧数publich边沿点，平面点以及全部点给laserMapping(每隔一帧发一次)
               if (frameCount >= skipFrameNum + 1) {
                    frameCount = 0;
                    //发送角点 每两帧发送一次
                    sensor_msgs::PointCloud2 laserCloudCornerLast2;
                    pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                    laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                    laserCloudCornerLast2.header.frame_id = "/camera";
                    pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
                    //发送平面点 每两帧发送一次
                    sensor_msgs::PointCloud2 laserCloudSurfLast2;
                    pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                    laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                    laserCloudSurfLast2.header.frame_id = "/camera";
                    pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
                    //发送所有点 每两帧发送一次
                    sensor_msgs::PointCloud2 laserCloudFullRes3;
                    pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
                    laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                    laserCloudFullRes3.header.frame_id = "/camera";
                    pubLaserCloudFullRes.publish(laserCloudFullRes3);
                }
        }//确保接收同一时刻消息

        status = ros::ok();
        //ros::Duration(0.5).sleep(); // sleep for half a second
        //度过剩下的时间
        rate.sleep();
    }//while(ros::ok())
    return 0;    
}

